#include "openniGraber.h"
#include "yamlConfig.h"
#include "frameProcess.h"

yamlHandler g_camre_config;
pcl::console::TicToc g_tt;

int main(int argc, char** argv)
{
    //读取配置文件
    std::string file_Name;
    if(argc > 1)
        file_Name = argv[1];
    else
        file_Name = "config/camreConfig.yaml";
    if(!g_camre_config._init_(file_Name))
    {
        std::cerr << "Cannot open config file, please check the path or file's format." << std::endl;
        exit(-1);
    }

    detectDevices();
    //深度相机类一定要在读取配置文件后再始化
    depthCapture cap_left(DEVICEID::DEVICE_ASTRA);
    depthCapture cap_right(DEVICEID::DEVICE_ASTRA_DEPPER);

    //初始化点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_XYZ(new pcl::PointCloud<pcl::PointXYZ>);
    cloud_XYZ->width = g_camre_config.width;
    cloud_XYZ->height = g_camre_config.height;
    cloud_XYZ->points.resize(cloud_XYZ->width * cloud_XYZ->height);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_XYZ_right(new pcl::PointCloud<pcl::PointXYZ>);
    cloud_XYZ_right->width = g_camre_config.width;
    cloud_XYZ_right->height = g_camre_config.height;
    cloud_XYZ_right->points.resize(cloud_XYZ_right->width * cloud_XYZ_right->height);
    //可视化
    //pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("viewer"));
    //pcl::visualization::PCLVisualizer::Ptr viewer_right(new pcl::visualization::PCLVisualizer("viewer_right"));
    //设置相机坐标系
    //viewer->setCameraPosition(0, 0, -2, 0, -1, 0, 0);
    //viewer_right->setCameraPosition(0, 0, -2, 0, -1, 0, 0);
    while(cap_left.isOpened() && cap_right.isOpened())
    {
        //*******//测试中，注释掉pclvisualizer的显示，测试帧率
        g_tt.tic();
        cap_left >> cloud_XYZ;
        //viewer->addPointCloud<pcl::PointXYZ>(cloud_XYZ, "cloud");
        cap_right >> cloud_XYZ_right;
        //viewer_right->addPointCloud<pcl::PointXYZ>(cloud_XYZ_right, "cloud");
        //viewer->spinOnce();
        //viewer_right->spinOnce();
        //更新下一帧前需要把当前帧去除
        //viewer->removeAllPointClouds();
        //viewer_right->removeAllPointClouds();
        float use_time = g_tt.toc();
        std::cout << "fps = " << 1000/use_time << "\n";
    }
    cap_left.shutdown();
    cap_right.shutdown();
    return 0;
}